#!/usr/bin/env python
PACKAGE = "se3_controller"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("kp_px", double_t, 0, "kp_px", 1.5, 0.0, 10.0)
gen.add("kp_py", double_t, 0, "kp_py", 1.5, 0.0, 10.0)
gen.add("kp_pz", double_t, 0, "kp_pz", 1.5, 0.0, 10.0)

gen.add("kp_vx", double_t, 0, "kp_vx", 1.5, 0.0, 10.0)
gen.add("kp_vy", double_t, 0, "kp_vy", 1.5, 0.0, 10.0)
gen.add("kp_vz", double_t, 0, "kp_vz", 1.5, 0.0, 10.0)

gen.add("kp_ax", double_t, 0, "kp_ax", 1.5, 0.0, 10.0)
gen.add("kp_ay", double_t, 0, "kp_ay", 1.5, 0.0, 10.0)
gen.add("kp_az", double_t, 0, "kp_az", 1.5, 0.0, 10.0)

gen.add("kp_qx", double_t, 0, "kp_qx", 5.5, 0.0, 10.0)
gen.add("kp_qy", double_t, 0, "kp_qy", 5.5, 0.0, 10.0)
gen.add("kp_qz", double_t, 0, "kp_qz", 0.1, 0.0, 10.0)

gen.add("kp_wx", double_t, 0, "kp_wx", 1.5, 0.0, 10.0)
gen.add("kp_wy", double_t, 0, "kp_wy", 1.5, 0.0, 10.0)
gen.add("kp_wz", double_t, 0, "kp_wz", 0.1, 0.0, 10.0)

gen.add("kd_px", double_t, 0, "kd_px", 0.0, 0.0, 10.0)
gen.add("kd_py", double_t, 0, "kd_py", 0.0, 0.0, 10.0)
gen.add("kd_pz", double_t, 0, "kd_pz", 0.0, 0.0, 10.0)

gen.add("kd_vx", double_t, 0, "kd_vx", 0.0, 0.0, 10.0)
gen.add("kd_vy", double_t, 0, "kd_vy", 0.0, 0.0, 10.0)
gen.add("kd_vz", double_t, 0, "kd_vz", 0.0, 0.0, 10.0)

gen.add("kd_ax", double_t, 0, "kd_ax", 0.0, 0.0, 10.0)
gen.add("kd_ay", double_t, 0, "kd_ay", 0.0, 0.0, 10.0)
gen.add("kd_az", double_t, 0, "kd_az", 0.0, 0.0, 10.0)

gen.add("kd_qx", double_t, 0, "kd_qx", 0.0, 0.0, 10.0)
gen.add("kd_qy", double_t, 0, "kd_qy", 0.0, 0.0, 10.0)
gen.add("kd_qz", double_t, 0, "kd_qz", 0.0, 0.0, 10.0)

gen.add("kd_wx", double_t, 0, "kd_wx", 0.0, 0.0, 10.0)
gen.add("kd_wy", double_t, 0, "kd_wy", 0.0, 0.0, 10.0)
gen.add("kd_wz", double_t, 0, "kd_wz", 0.0, 0.0, 10.0)

gen.add("limit_err_p", double_t, 0, "limit_err_p", 1.0, 0.0, 10.0) 
gen.add("limit_err_v", double_t, 0, "limit_err_v", 1.0, 0.0, 10.0)
gen.add("limit_err_a", double_t, 0, "limit_err_a", 1.0, 0.0, 10.0)

gen.add("limit_d_err_p", double_t, 0, "limit_d_err_p", 1.0, 0.0, 10.0)
gen.add("limit_d_err_v", double_t, 0, "limit_d_err_v", 1.0, 0.0, 10.0)
gen.add("limit_d_err_a", double_t, 0, "limit_d_err_a", 1.0, 0.0, 10.0)

gen.add("desire_px", double_t, 0, "desire_px", 0.0, -1, 1)
gen.add("desire_py", double_t, 0, "desire_py", 0.0, -2, 2)
gen.add("desire_pz", double_t, 0, "desire_pz", 1.0, -0.5, 2)

gen.add("desire_roll", double_t, 0, "desire_roll", 0.0, 0.0, 0.0)
gen.add("desire_pitch", double_t, 0, "desire_pitch", 0.0, 0.0, 0.0)
gen.add("desire_yaw", double_t, 0, "desire_yaw", 0.0, -3.14, 3.14)

exit(gen.generate(PACKAGE, "se3_controller", "se3_dynamic_tune"))